package com.hitqz.robot.driver;

import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.fastjson.JSON;
import com.google.common.collect.Lists;
import com.hitqz.robot.api.business.enums.FormConfigEnum;
import com.hitqz.robot.api.business.enums.RobotDriverEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.business.enums.RobotParamsFormFieldEnum;
import com.hitqz.robot.api.common.dto.form.FormConfigDto;
import com.hitqz.robot.api.common.dto.form.FormFieldConfigDto;
import com.hitqz.robot.api.common.dto.robot.InitPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.dto.warning.AddWarningDto;
import com.hitqz.robot.api.common.entity.Robot;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.dto.*;
import com.hitqz.robot.driver.dto.robot.RobotCustomInfo;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.model.MapAttributesDto;
import com.hitqz.robot.driver.model.PointDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.hitqz.robot.driver.util.TrackCacheUtil;
import com.hitqz.robot.driver.util.TrackToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Service;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.*;
import java.util.stream.Collectors;

@Slf4j
@Service("robotDriverService")
public class HitTrackRobotDriverImpl implements RobotDriverService {

    private static short speed = 20;
    // cm/s
    private static final short minSpeed = 10;
    // cm/s
    private static final short maxSpeed = 50;

    @Override
    public boolean doOnlineMode(Robot robot) {
        boolean result = false;
        if (StrUtil.isNotEmpty(TrackToolkit.serialNumber)) {
            return true;
        }
        try {
            Integer port = null;
            if (robot.getIpAddress().contains(":")) {
                port = Integer.parseInt(robot.getIpAddress().split(":")[1]);
            }
            TrackToolkit.serialNumber = robot.getRobotSn();
            result = TrackToolkit.init(port);
        }catch (Exception e){
            log.error("[track] doOnlineMode error",e);
        }
        log.info("[track] doOnlineMode success");
        return result;
    }

    @Override
    public boolean setSpeed(RobotMoveDto baseSpeedDto) {
        //改变标志位
        TrackCacheUtil.setSpeedFlag();
        String linerSpeed = String.valueOf(speed);
        if (StrUtil.isNotBlank(baseSpeedDto.getLinerSpeed())) {
            short speed = baseSpeedDto.getLinerSpeed().contains("-") ? (short) ((-1) * Short.parseShort(linerSpeed)) : Short.parseShort(linerSpeed);
            speed = Objects.equals(baseSpeedDto.getLinerSpeed(),"0")? 0 : speed;
            TrackToolkit.setSpeed(speed, (short) 0);
            log.info("[track] set line speed {} cm/s", speed);
        }
        return true;
    }

    @Override
    public boolean changeWebControlSpeed(float changeValue) {
        //changeValue 是 m/s 需要换算成 cm/s
        short changeValueShort = 10;
        if (changeValue<0.0f) {
            changeValueShort = -10;
        }
        speed = (short) Math.min(Math.max(speed + changeValueShort,minSpeed), maxSpeed);
        return true;
    }

    @Override
    public boolean setInitPos(InitPosDto baseInitPosDto) {
        return true;
    }

    @Override
    public boolean robotTaskGoTarget(RobotGoTargetPosDto dto) {
        //rfid下code 命名规则 坞站站点 0 坞站铁块 0 第一点铁块正 10 第一点站点 10,11 第一点铁块反11  第二点铁块正 20 第一点站点 20,21 第一点铁块反21
        //里程计下code 命名规则 坞站站点 0  第一点站点 10 第二点站点 20
        String code = dto.getCode();
        String targetCode;
        if (StrUtil.isBlank(code)){
            return false;
        }
        //记录 用来映射 rfid模式下的站点
        TrackToolkit.currentTargetCode = code;
        //code不是 short 返回 false
        LocateStatusDto locationStatus = TrackToolkit.getLocationStatus();
        byte status = locationStatus.getLocatedStatus();
        short lastId = locationStatus.getLastId();
        short nextId = locationStatus.getNextId();
        String[] codes = Arrays.stream(code.split("-")).filter(StrUtil::isNotBlank).toArray(String[]::new);
        //fixme 可以放宽限制
        if (codes.length>2){
            log.info("[track]编码错误,最多 2 个数字");
            return false;
        }
        int lastCode = -1;
        for (int i = 0; i < codes.length; i++) {
            String s = codes[i];
            if (!StrUtil.isNumeric(s)) {
                log.warn("[track]编码格式有误");
                return false;
            }
            if (lastCode>Short.parseShort(s)){
                log.warn("[track]编码没有按从小到大");
                return false;
            }
            lastCode = Short.parseShort(s);
        }
        //站点左侧
        if (lastId<=Short.parseShort(codes[0])){
            targetCode = codes[0];
            log.info("[track]在站点左侧");
        } else if (lastId <= Short.parseShort(codes[Math.min(1,codes.length-1)])){
            targetCode = codes[Math.min(1,codes.length-1)];
            log.info("[track]在站点中");
        } else {
            targetCode = codes[Math.min(1,codes.length-1)];
            log.info("[track]在站点右侧");
        }
        if (lastId == Short.parseShort(targetCode) && status == (byte) 2){
            log.info("[track]已在站点");
            return true;
        }
        //如果在坞站,取消充电
        if (lastId==nextId && lastId==(short) 0 && !Objects.equals(targetCode,"0")){
            this.cmdCharge(0);
            log.info("[track]出坞站,取消充电");
            try {
                Thread.sleep(500);
            } catch (InterruptedException e) {
                throw new RuntimeException(e);
            }
        }
        return TrackToolkit.goPos(Short.parseShort(targetCode));
    }

    @Override
    public boolean motionAbort() {
        RobotMoveDto robotMoveDto = new RobotMoveDto();
        robotMoveDto.setLinerSpeed("0");
        this.setSpeed(robotMoveDto);
        return true;
    }

    @Override
    public boolean navCmd(Integer cmd) {
        return true;
    }

    @Override
    public boolean cmdCharge(Integer cmd) {
        LocateStatusDto locationStatus = TrackToolkit.getLocationStatus();
        byte status = locationStatus.getLocatedStatus();
        int mileage = locationStatus.getMileage();
        short lastId = locationStatus.getLastId();
        short nextId = locationStatus.getNextId();
        if (cmd == 1 && (lastId!=0 || nextId!=0 || status!=(byte)2) && !(TrackToolkit.controlMode== (byte)2 && Math.abs(mileage)<10)){
            AddWarningDto addWarningDto = new AddWarningDto();
            addWarningDto.setTitle("充电失败");
            addWarningDto.setContent("机器人不在坞站,禁止下发充电命令");
            addWarningDto.setCode("");
            addWarningDto.setSuggestion("确保机器人在坞站,再重新下发充电");
            addWarningDto.setLevel(0);
            SpringUtil.getApplicationContext().publishEvent(addWarningDto);
            return false;
        }
        return TrackToolkit.setChargeStatus(cmd.byteValue());
    }


    @Override
    public IRobotPosDto getWebPos() {
        IRobotPosDto iRobotPosDto = new IRobotPosDto();
        iRobotPosDto.setUrl(Optional.ofNullable(TrackToolkit.map).map(RobotMap::getFilePath).orElse(""));
        iRobotPosDto.setCurrentMap(Optional.ofNullable(TrackToolkit.map).map(RobotMap::getMapCode).orElse(""));
        iRobotPosDto.setWebControlSpeed(BigDecimal.valueOf(speed).divide(BigDecimal.valueOf(100),2, RoundingMode.HALF_UP).floatValue());
        if (TrackToolkit.ctx==null){
            return iRobotPosDto;
        }
        LocateStatusDto locationStatus = TrackToolkit.getLocationStatus();
        PowerInfoDto powerInfo = TrackToolkit.getPowerInfo();
        CalibrationPointDto calibrationPointInfo = TrackToolkit.getCalibrationPointInfo();
        StopStatusDto stopStatus = TrackToolkit.getStopStatus();
        CalculateLocateDto calculateLocateDto = Optional.ofNullable((CalculateLocateDto) TrackCacheUtil.get(TrackCacheUtil.KEY_CALCULATE_LOCATION)).orElse(CalculateLocateDto.builder().build());
        iRobotPosDto.setControllerTemp(0.0F);
        iRobotPosDto.setX(locationStatus.getMileage());
        iRobotPosDto.setY(0);
        iRobotPosDto.setCurrentStation(String.valueOf(locationStatus.getLastId()));
        iRobotPosDto.setVx(BigDecimal.valueOf(locationStatus.getSpeed()).divide(BigDecimal.valueOf(100),2, RoundingMode.HALF_UP).floatValue());
        iRobotPosDto.setBlocked(stopStatus.getUltrasonic()==1 );
        iRobotPosDto.setBatteryLevel(powerInfo.getPercentage());
        iRobotPosDto.setCharging(powerInfo.getCurrentDirection()==1);
        iRobotPosDto.setEmergency(stopStatus.getHardStop()==1 || stopStatus.getStopBar()==1);
        iRobotPosDto.setCurrentDirection(powerInfo.getCurrentDirection() == 0 ? "放电": "充电" );
        iRobotPosDto.setVoltage(powerInfo.getVoltage());
        iRobotPosDto.setCurrent(String.valueOf(powerInfo.getCurrent()));
        //速度正值,改单位为m/s,前端展示用
        iRobotPosDto.setSpeed(BigDecimal.valueOf(Math.abs(locationStatus.getSpeed())).divide(BigDecimal.valueOf(100),2,RoundingMode.HALF_UP).floatValue());
        iRobotPosDto.setPixelAngle(0.0F);
        iRobotPosDto.setPassedPath(Lists.newArrayList());
        iRobotPosDto.setTaskStatusStr("");
        iRobotPosDto.setLabel(calibrationPointInfo.getRfid());
        //换算位置信息
        iRobotPosDto.setPixelX(calculateLocateDto.getPixelX());
        iRobotPosDto.setPixelY(calculateLocateDto.getPixelY());
        iRobotPosDto.setReached(calculateLocateDto.isReach());
        iRobotPosDto.setCurrentStation(calculateLocateDto.getCurrentStation());
        return iRobotPosDto;
    }

    @Override
    public List<RobotMapPos> getNavPath(RobotMapPos sourcePos, RobotMapPos targetPos, List<RobotMapPos> allRobotMapPos) {
        return List.of();
    }

    @Override
    public List<RobotMapPos> getRobotMapPos(String mapCode) {
        return List.of();
    }

    @Override
    public List<RobotMap> getMapList() {
        return List.of();
    }

    @Override
    public String getCurrentMap() {
        return "";
    }

    @Override
    public MapAttributesDto getMapAttributes(String mapCode) {
        return null;
    }

    @Override
    public void selMap(RobotMap map) {
        log.info("[track]sel map:{}",map.getMapCode());
        TrackToolkit.map = map;
        TrackToolkit.posMap = map.getPosList().stream().collect(Collectors.toMap(RobotMapPos::getCode, pos -> pos));
    }

    @Override
    public PointDto canvas2Point(RobotMap robotMap, PointDto pointDto) {
        return pointDto;
    }

    @Override
    public FormConfigDto getRobotConfig(String formName) {
        // 创建字段配置
        ArrayList<FormFieldConfigDto> fields = new ArrayList<>();
        // 传感器使能
        fields.add(RobotParamsFormFieldEnum.ULTRASONIC_ENABLE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.COLLISION_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.DROP_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.INFRARED_AVOIDANCE_ENABLED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.SAFETY_AVOIDANCE_ENABLED.toConfigDto());
        // 传感器参数
        fields.add(RobotParamsFormFieldEnum.FRONT_ULTRASONIC_TRIGGER_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.FRONT_ULTRASONIC_RELEASE_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.BACK_ULTRASONIC_TRIGGER_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.BACK_ULTRASONIC_RELEASE_DISTANCE.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.CHARGER_IP.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.CHARGER_PORT.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RELAY_IP.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RELAY_PORT.toConfigDto());
        // 机器人参数
        fields.add(RobotParamsFormFieldEnum.TRACK_LOCATE_METHOD.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.ROBOT_SPEED.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.ROBOT_MILEAGE_CALIBRATION_FACTOR.toConfigDto());
        fields.add(RobotParamsFormFieldEnum.RFID_READER_POWER.toConfigDto());
        // 通过 FormConfigEnum 创建表单配置
        return FormConfigEnum.ROBOT_PARAMS_CONFIG.toFormConfigDto(fields);
    }


    @Override
    public R executeOtherCommand(RobotOtherCommandEnum cmdEnum,String jsonParams) {
        boolean result = true;
        switch (cmdEnum){
            case BATCH_CALIBRATION -> {
                List<RobotMapPos> pos = JSON.parseArray(jsonParams, RobotMapPos.class);
                //rfid 标定的 要排除掉 rfid 值是0或者空的
                if (Objects.equals(TrackToolkit.locationMethod,"rfid")) {
                    pos = pos.stream().filter(p->
                                    StrUtil.isNotBlank(p.getLabel())
                                    &&!Objects.equals(p.getLabel(),"0")
                                    &&!Objects.equals(p.getLabel(),"000000000000000000000000"))
                            .toList();
                }
                //每 3个进行一次标定
                for (int i = 0; i < pos.size(); i += 3) {
                    List<RobotMapPos> subList = pos.subList(i, Math.min(i + 3, pos.size()));
                    List<AddCalibrationPointDto> list = new ArrayList<>();
                    for (RobotMapPos robotMapPos : subList) {
                        AddCalibrationPointDto addDto = AddCalibrationPointDto.builder()
                                .rfid(robotMapPos.getLabel())
                                .mileage(Double.valueOf(robotMapPos.getPosX()).intValue())
                                .number(Short.parseShort(robotMapPos.getCode()))
                                .isStart(Short.parseShort(robotMapPos.getCode()) == 0 ? (byte) 1 : (byte) 0)
                                .build();
                        list.add(addDto);
                    }
                    if (Objects.equals(TrackToolkit.locationMethod,"rfid")) {
                        result = result & TrackToolkit.setCalibrationPointBatch(list);
                    }
                }
            }
            case CLEAR_CALIBRATION -> {
                result = TrackToolkit.clearCalibrationPointAll();
            }
            case POWER_ON_OR_OFF -> {
                result = TrackToolkit.remotePowerOnOrOff();
            }
            case SPEAK_REQUEST -> {
                return R.ok(String.valueOf(TrackToolkit.speakerReqStatus));
            }
            default -> {
                log.warn("[track]other unknown cmd");
            }
        }
        return result ? R.ok() : R.failed();
    }

    @Override
    public List<RobotCustomInfo> getCustomInfo(){
        List<RobotCustomInfo> customInfo = new ArrayList<RobotCustomInfo>();
        AirQualityDto airQuality = Optional.ofNullable((AirQualityDto) TrackCacheUtil.get(TrackCacheUtil.KEY_AIR_QUALITY)).orElse(AirQualityDto.builder().build());
        customInfo.add(RobotCustomInfo.create("gas", "可燃气", String.valueOf(airQuality.getGas())));
        customInfo.add(RobotCustomInfo.create("totalMileage", "累计里程", TrackToolkit.totoMileage.get() +"mm"));
        return customInfo;
    }

    @Override
    public RobotDriverEnum getDriverVersion() {
        return RobotDriverEnum.TRACK;
    }
}
